#include "PhysicsLayer.h"

#define GRABABLE_MASK_BIT (1<<31)

CPhysicsLayer::CPhysicsLayer()
{
	// enable events
	setTouchEnabled(true);
	setAccelerometerEnabled(true);

	CCSize s = CCDirector::sharedDirector()->getWinSize();

	// init physics
	initPhysics();

	scheduleUpdate();
}

CPhysicsLayer::~CPhysicsLayer()
{
	// manually Free rogue shapes
	for( int i=0;i<4;i++) {
		cpShapeFree( m_pWalls[i] );
	}

	cpSpaceFree( m_pSpace );

}

void CPhysicsLayer::initPhysics()
{
	CCSize s = CCDirector::sharedDirector()->getWinSize();

	// init chipmunk
	//cpInitChipmunk();

	m_pSpace = cpSpaceNew();

	m_pSpace->gravity = cpv(0, -240);

	//
	// rogue shapes
	// We have to free them manually
	//
	// bottom
	m_pWalls[0] = cpSegmentShapeNew( m_pSpace->staticBody, cpv(0,0), cpv(s.width,0), 0.0f);

	// top
	m_pWalls[1] = cpSegmentShapeNew( m_pSpace->staticBody, cpv(0,s.height), cpv(s.width,s.height), 0.0f);

	// left
	m_pWalls[2] = cpSegmentShapeNew( m_pSpace->staticBody, cpv(0,0), cpv(0,s.height), 0.0f);

	// right
	m_pWalls[3] = cpSegmentShapeNew( m_pSpace->staticBody, cpv(s.width,0), cpv(s.width,s.height), 0.0f);

	for( int i=0;i<4;i++) {
		m_pWalls[i]->e = 1.0f;
		m_pWalls[i]->u = 1.0f;
		cpSpaceAddStaticShape(m_pSpace, m_pWalls[i] );
	}

	m_pMouseBody = cpBodyNew(100.0,INFINITY);
	m_pMouseJoint = NULL;
}

void CPhysicsLayer::update(float delta)
{
	// Should use a fixed size step based on the animation interval.
	int steps = 2;
	float dt = CCDirector::sharedDirector()->getAnimationInterval()/(float)steps;

	for(int i=0; i<steps; i++){
		cpSpaceStep(m_pSpace, dt);
	}
}

void CPhysicsLayer::reset(CCObject* sender)
{
	CCScene* s = new CCScene();
	CPhysicsLayer* child = new CPhysicsLayer();
	s->addChild(child);
	child->release();
	CCDirector::sharedDirector()->replaceScene(s);
	s->release();
}

void CPhysicsLayer::addNewSpriteAtPosition(CCSprite* pSprite,CCPoint pos)
{
	CCSpriteBatchNode* pNode = InitSprite("Cloud.png");
		
	CPhiysicsSprite* Sprite = new CPhiysicsSprite;

	CCSize size = pNode->getTexture()->getContentSize();
	Sprite->initWithTexture(pNode->getTexture());
	Sprite->autorelease();
	Sprite->setScale(0.9);

	pNode->addChild(Sprite);

	Sprite->setPosition(pos);

	int num = 4;

	cpVect verts[] = {
		cpv(0-size.width/2, 0-size.height/2),
		cpv(0-size.width/2, size.height/2),
		cpv( size.width/2, size.height/2),
		cpv( size.width/2,0-size.height/2),
	};
	
	float mass = 3.0f;
	cpBody *body = cpBodyNew(mass, cpMomentForPoly(mass, num, verts, cpvzero));
	
	body->p = cpv(pos.x, pos.y);
	cpSpaceAddBody(m_pSpace, body);

	cpShape* shape = cpPolyShapeNew(body, num, verts, cpvzero);

	shape->e = 0.5f; shape->u = 0.5f;
	cpSpaceAddShape(m_pSpace, shape);
	Sprite->setPhysicsBody(body);
}

void CPhysicsLayer::addBatchNodeAtPosition(CCSpriteBatchNode* pNode, CCPoint pos)
{
	CPhiysicsSprite* Sprite = new CPhiysicsSprite;

	CCSize size = m_pSpriteTexture->getContentSize();
	Sprite->initWithTexture(pNode->getTexture());
	Sprite->autorelease();
	Sprite->setScale(0.1);

	pNode->addChild(Sprite, 0, 13);

	Sprite->setPosition(pos);

	float mass = 100.0f;

	m_pBallBody = cpBodyNew(mass, cpMomentForCircle(mass, 0.0, size.width/2, cpvzero));
	m_pBallBody->p = cpv(pos.x, pos.y);
	cpSpaceAddBody(m_pSpace, m_pBallBody);

	cpShape* shape = cpCircleShapeNew(m_pBallBody, size.width/2, cpvzero);
	shape->e = 0.5f; shape->u = 0.5f;
	cpSpaceAddShape(m_pSpace, shape);
	Sprite->setPhysicsBody(m_pBallBody);
}

void CPhysicsLayer::onEnter()
{
	CCLayer::onEnter();
}

void CPhysicsLayer::ccTouchesBegan(CCSet* touches, CCEvent* event)
{
	CCSetIterator it;
	CCTouch* touch;
	
	for( it = touches->begin(); it != touches->end(); it++) 
	{
		touch = (CCTouch*)(*it);

		if(!touch)
			break;

		CCPoint location = touch->getLocation();
		cpVect point = cpv(location.x, location.y);
		
		cpShape *shape = cpSpacePointQueryFirst(m_pSpace, point, GRABABLE_MASK_BIT,0);

		if(shape) {
			m_pMouseBody->p = point;
			m_pMouseJoint = cpPivotJointNew2(m_pMouseBody, shape->body, cpvzero, cpBodyWorld2Local(shape->body, point));
			cpSpaceAddConstraint(m_pSpace, m_pMouseJoint);
		}
		else
		{
			CCPoint location = touch->getLocation();
			//addNewSpriteAtPosition( location );
		}
	}
}

void CPhysicsLayer::ccTouchesMoved(CCSet* touches, CCEvent* event)
{
	CCSetIterator it;
	CCTouch* touch;

	for( it = touches->begin(); it != touches->end(); it++) 
	{
		touch = (CCTouch*)(*it);

		if(!touch)
			break;

		CCPoint location = touch->getLocation();
		cpVect point = cpv(location.x, location.y);
		m_pMouseBody->p = point;

	}
}


void CPhysicsLayer::ccTouchesEnded(CCSet* touches, CCEvent* event)
{
	//Add a new body/atlas sprite at the touched location
	CCSetIterator it;
	CCTouch* touch;

	for( it = touches->begin(); it != touches->end(); it++) 
	{
		touch = (CCTouch*)(*it);

		if(!touch)
			break;
		if (m_pMouseJoint)
		{
			cpSpaceRemoveConstraint(m_pSpace,m_pMouseJoint);
			m_pMouseJoint = NULL;
		}
	}
}

void CPhysicsLayer::didAccelerate(CCAcceleration* pAccelerationValue)
{
	static float prevX=0, prevY=0;

#define kFilterFactor 0.05f

	float accelX = (float) pAccelerationValue->x * kFilterFactor + (1- kFilterFactor)*prevX;
	float accelY = (float) pAccelerationValue->y * kFilterFactor + (1- kFilterFactor)*prevY;

	prevX = accelX;
	prevY = accelY;

	CCPoint v = ccp( accelX, accelY);
	v = ccpMult(v, 200);
	m_pSpace->gravity = cpv(v.x, v.y);
}

CCSpriteBatchNode* CPhysicsLayer::InitSprite(const char* File)
{
	CCSpriteBatchNode* pNodeSprite = CCSpriteBatchNode::create(File);
	m_pSpriteTexture = pNodeSprite->getTexture();
	addChild(pNodeSprite, 0, 1);
	return pNodeSprite;
}

void CPhysicsLayer::MoveBatchNodeToLeft(CCSpriteBatchNode* pNode)
{
	//CPhiysicsSprite* PhSprite = (CPhiysicsSprite*)pNode->getChildByTag(13);
	cpBodyApplyForce(m_pBallBody,cpv(-3000,0),cpvzero);
}

void CPhysicsLayer::MoveBatchNodeToRight(CCSpriteBatchNode* pNode)
{
	cpBodyApplyForce(m_pBallBody,cpv(3000,0),cpvzero);
}

void CPhysicsLayer::AddNewPhysicsObject(CCSprite* pSprite)
{
	CCPoint pos = pSprite->getPosition();



	CCSize size = pSprite->getTexture()->getContentSize();

	CCSpriteBatchNode* pNode = CCSpriteBatchNode::batchNodeWithTexture(pSprite->getTexture());

	CPhiysicsSprite* Sprite = new CPhiysicsSprite;
	
	Sprite->initWithTexture(pNode->getTexture());
	Sprite->autorelease();
	
	pNode->addChild(Sprite);

	Sprite->setPosition(pos);

	int num = 4;

	cpVect verts[] = {
		cpv(0-size.width/2, 0-size.height/2),
		cpv(0-size.width/2, size.height/2),
		cpv( size.width/2, size.height/2),
		cpv( size.width/2,0-size.height/2),
	};
	
	float mass = 3.0f;
	cpBody *body = cpBodyNew(mass, cpMomentForPoly(mass, num, verts, cpvzero));
	
	body->p = cpv(pos.x, pos.y);
	cpSpaceAddBody(m_pSpace, body);

	cpShape* shape = cpPolyShapeNew(body, num, verts, cpvzero);

	shape->e = 0.5f; shape->u = 0.5f;
	cpSpaceAddShape(m_pSpace, shape);
	Sprite->setPhysicsBody(body);

	addChild(pNode, 0, 1);
}